// -*- coding: utf-8 -*-
// MCU数据订阅器使用示例
#include "mcu_data_subscriber.h"
#include "mcu_data_sender.h"
#include <ros/ros.h>
#include <locale>
#include <codecvt>

class ExampleNode {
public:
    ExampleNode(ros::NodeHandle& nh) 
        : nh_(nh), mcuSubscriber_(nh), mcuDataSender_(nh) {
        
        ROS_INFO("=== MCU数据订阅器示例节点启动 ===");
        
        // 示例1: 同步方式获取数据
        setupSyncExample();
        
        // 示例2: 异步回调方式
        setupAsyncExample();
        
        // 等待一下让订阅器建立连接
        ros::Duration(1.0).sleep();
    }
    
    void runExample() {
        ros::Rate rate(2); // 2Hz
        int counter = 0;
        
        while (ros::ok()) {
            counter++;
            
            // 每5秒执行一次同步数据获取示例
            if (counter % 10 == 0) {
                demonstrateSyncUsage();
            }
            
            // 每3秒发送一个测试命令
            if (counter % 6 == 0) {
                sendTestCommands();
            }
            
            // 每10秒打印当前所有数据状态
            if (counter % 20 == 0) {
                mcuSubscriber_.printAllData();
            }
            
            ros::spinOnce();
            rate.sleep();
        }
    }

private:
    ros::NodeHandle& nh_;
    McuDataSubscriber mcuSubscriber_;
    McuDataSender mcuDataSender_;
    
    void setupSyncExample() {
        ROS_INFO("设置同步数据获取示例...");
        
        // 为了同步获取数据，我们需要先订阅这些话题
        // 使用空的回调函数就可以了
        mcuSubscriber_.subscribeDouble("sensor_voltage", [](double value) {
            // 这里可以为空，我们主要用同步接口获取数据
        });
        
        mcuSubscriber_.subscribeDouble("sensor_temperature", [](double value) {
            // 空回调
        });
        
        mcuSubscriber_.subscribeString("device_status", [](const std::string& value) {
            // 空回调
        });
    }
    
    void setupAsyncExample() {
        ROS_INFO("设置异步回调示例...");
        
        // 订阅按钮状态，使用回调函数
        mcuSubscriber_.subscribeButton("button_basic", [this](bool pressed) {
            if (pressed) {
                ROS_INFO("🔴 基础按钮被按下！执行相应动作...");
                // 响应按钮按下，发送LED控制命令
                mcuDataSender_.sendJsonKeyValue("led_mode", "button_response");
                mcuDataSender_.sendJsonKeyValue("led_brightness", 100);
            } else {
                ROS_INFO("🔵 基础按钮释放");
            }
        });
        
        mcuSubscriber_.subscribeButton("button_adv", [this](bool pressed) {
            if (pressed) {
                ROS_INFO("🟡 高级按钮被按下！");
                mcuDataSender_.sendJsonKeyValue("system_mode", "advanced");
            }
        });
        
        mcuSubscriber_.subscribeButton("button_cam_calib", [this](bool pressed) {
            if (pressed) {
                ROS_INFO("📷 相机校准按钮被按下！");
                mcuDataSender_.sendJsonKeyValue("start_calibration", true);
            }
        });
        
        // 订阅传感器数据，监控异常情况
        mcuSubscriber_.subscribeDouble("sensor_voltage", [this](double voltage) {
            ROS_DEBUG("电压更新: %.2f V", voltage);
            
            if (voltage < 11.0) {
                ROS_WARN("⚠️  电压过低警告: %.2f V", voltage);
                mcuDataSender_.sendJsonKeyValue("power_warning", true);
                mcuDataSender_.sendJsonKeyValue("emergency_mode", true);
            } else if (voltage > 14.0) {
                ROS_WARN("⚠️  电压过高警告: %.2f V", voltage);
                mcuDataSender_.sendJsonKeyValue("voltage_warning", true);
            }
        });
        
        mcuSubscriber_.subscribeDouble("sensor_temperature", [this](double temp) {
            ROS_DEBUG("温度更新: %.1f °C", temp);
            
            if (temp > 80.0) {
                ROS_ERROR("🔥 温度过高警告: %.1f °C", temp);
                mcuDataSender_.sendJsonKeyValue("overheat_warning", true);
                mcuDataSender_.sendJsonKeyValue("cooling_fan", true);
            }
        });
        
        // 订阅设备状态，记录重要信息
        mcuSubscriber_.subscribeString("device_status", [](const std::string& status) {
            ROS_INFO("📊 设备状态更新: %s", status.c_str());
        });
        
        mcuSubscriber_.subscribeString("error_message", [](const std::string& error) {
            if (!error.empty()) {
                ROS_ERROR("❌ MCU错误消息: %s", error.c_str());
            }
        });
    }
    
    void demonstrateSyncUsage() {
        ROS_INFO("=== 同步数据获取示例 ===");
        
        // 检查数据是否可用
        if (mcuSubscriber_.hasData("sensor_voltage")) {
            double voltage = mcuSubscriber_.getDoubleValue("sensor_voltage");
            ros::Time lastUpdate = mcuSubscriber_.getLastUpdateTime("sensor_voltage");
            double ageSeconds = (ros::Time::now() - lastUpdate).toSec();
            
            ROS_INFO("当前电压: %.2f V (%.1f秒前更新)", voltage, ageSeconds);
        } else {
            ROS_WARN("电压数据暂时不可用");
        }
        
        if (mcuSubscriber_.hasData("sensor_temperature")) {
            double temp = mcuSubscriber_.getDoubleValue("sensor_temperature");
            ROS_INFO("当前温度: %.1f °C", temp);
        }
        
        if (mcuSubscriber_.hasData("device_status")) {
            std::string status = mcuSubscriber_.getStringValue("device_status");
            ROS_INFO("设备状态: %s", status.c_str());
        }
        
        // 检查按钮状态
        bool basicPressed = mcuSubscriber_.getButtonState("button_basic");
        bool advPressed = mcuSubscriber_.getButtonState("button_adv");
        bool camPressed = mcuSubscriber_.getButtonState("button_cam_calib");
        
        ROS_INFO("按钮状态 - 基础:%s, 高级:%s, 相机:%s", 
                 basicPressed ? "按下" : "释放",
                 advPressed ? "按下" : "释放", 
                 camPressed ? "按下" : "释放");
        
        ROS_INFO("========================");
    }
    
    void sendTestCommands() {
        static int commandCounter = 0;
        commandCounter++;
        
        ROS_INFO("发送测试命令 #%d", commandCounter);
        
        // 发送一些测试命令到MCU
        switch (commandCounter % 4) {
            case 0:
                mcuDataSender_.sendJsonKeyValue("led_brightness", 50 + (commandCounter % 50));
                mcuDataSender_.sendJsonKeyValue("query", "all_sensors");
                break;
            case 1:
                mcuDataSender_.sendJsonKeyValue("motor_speed", 1000 + (commandCounter % 500));
                break;
            case 2:
                mcuDataSender_.sendJsonKeyValue("system_mode", 
                    (commandCounter % 2 == 0) ? "auto" : "manual");
                break;
            case 3:
                mcuDataSender_.sendJsonKeyValue("heartbeat", commandCounter);
                mcuDataSender_.sendJsonKeyValue("request_status", true);
                break;
        }
    }
};

int main(int argc, char** argv) {
    // 设置中文环境
    std::setlocale(LC_ALL, "zh_CN.UTF-8");
    
    // 初始化ROS节点
    ros::init(argc, argv, "mcu_subscriber_example");
    ros::NodeHandle nh;
    
    ROS_INFO("启动MCU数据订阅器示例程序");
    ROS_INFO("请确保 serialport_node 已经在运行");
    ROS_INFO("使用命令: rosrun pegasus serialport_node");
    
    try {
        ExampleNode node(nh);
        
        ROS_INFO("示例程序已就绪，开始演示...");
        ROS_INFO("程序会展示:");
        ROS_INFO("1. 异步回调方式处理按钮和传感器数据");
        ROS_INFO("2. 同步方式主动查询数据");
        ROS_INFO("3. 向MCU发送JSON命令");
        ROS_INFO("4. 定期打印所有数据状态");
        
        node.runExample();
        
    } catch (const std::exception& e) {
        ROS_ERROR("示例程序异常: %s", e.what());
        return -1;
    }
    
    ROS_INFO("示例程序退出");
    return 0;
}
